# -*- coding: utf-8 -*-

from my_simulation import MySimulatedIRSensor, MySystem

import quantities
import signal
import time
import sys
import random

import vtk
from vtk.util.colors import red, green, blue
from abstract_sensor.definition import VTKSmartAbstractSensor
from auxillaries.check_observability import CheckObservability

def Keypress(obj, event):
    global actMeas
    global iren
    global measurement
    global position
    global orientation
    global beamMinMax
    
    key = obj.GetKeySym()
    if key== "X":
        measurement[0]=measurement[0]+1
    if key== "x":
        measurement[0]=measurement[0]-1
    if key== "Y":
        measurement[1]=measurement[1]+1
    if key== "y":
        measurement[1]=measurement[1]-1 
    if key== "Z":
        measurement[2]=measurement[2]+1
    if key== "z":
        measurement[2]=measurement[2]-1 

    inside=CheckObservability.sim_test(sensorPos=position, 
                                    sensorOri=orientation,
                                    beam=beamMinMax,
                                    measurement=measurement)
    actMeas.SetPosition(measurement)
    if inside==True:
        actMeas.GetProperty().SetColor(green)
        print "Measurement", measurement, "inside of the beam !"
    else:
        actMeas.GetProperty().SetColor(red)
        print "Measurement", measurement, "outside of the beam !"
    iren.GetRenderWindow().Render()

class MyAbstractSensorNode(VTKSmartAbstractSensor):
    
    def __init__(self, outputDataSheet, sensor_interface, appName, outputLayer=[], logger=[]):
        VTKSmartAbstractSensor.__init__(self, 
                                     outputDataSheet=outputDataSheet,
                                     sensor_interface=sensor_interface,
                                     appName = appName)
        
    def input_event_mapping(self, value, time=[]):
        '''
        The user have to map the incoming sensor measurements to the input event format
        Here MOSAIC ensures that only valid information (correct size, unit, etc.) are
        integrated.
        '''
        self.input_layer.input_event.set_value(value*quantities.meter, 'Measurement')
        self.input_layer.input_event.set_value([0.5]*quantities.centimeter**2, 'Uncertainty')   

    
    def output_event_mapping(self, results, common_validity):
        meas=self.input_events[0].get_value('Measurement')
        self.output_event.set_value(meas, 'Measurement')
        self.output_event.set_value(5.0*quantities.second, 'Timestamp')
        self.output_event.set_value([0.5]*quantities.centimeter**2, 'Uncertainty')
        self.output_event.set_value([100, 200, 0]*quantities.centimeter, 'Position')
        self.output_event.set_value([100, 200, 0]*quantities.degree, 'Orientation') 
        self.output_event.set_value([common_validity], 'Validity')        


if __name__ == "__main__":
    
    ren = vtk.vtkRenderer()
    renWin = vtk.vtkRenderWindow()
    renWin.AddRenderer(ren)
    iren = vtk.vtkRenderWindowInteractor()
    iren.SetRenderWindow(renWin)
  
    srcPlane = vtk.vtkPlaneSource()
    srcPlane.SetOrigin(-120, 120, -5)
    srcPlane.SetPoint1(120, 120, -5)
    srcPlane.SetPoint2(-120, -120, -5)
    srcPlane.SetResolution(10, 10)
        
    mapPlane = vtk.vtkPolyDataMapper();
    mapPlane.SetInput(srcPlane.GetOutput())
        
    actPlane = vtk.vtkActor()
    actPlane.SetMapper(mapPlane)
        
    srcCircle = vtk.vtkRegularPolygonSource()
    srcCircle.SetRadius(100)
    srcCircle.SetNumberOfSides(50)
    srcCircle.GeneratePolygonOff()
        
    mapCircle = vtk.vtkPolyDataMapper();
    mapCircle.SetInput(srcCircle.GetOutput())
        
    actCircle = vtk.vtkActor()
    actCircle.SetMapper(mapCircle)
    actCircle.SetPosition(0, 0, 0.1)
    actCircle.GetProperty().SetColor(1, 0, 0)

    axes = vtk.vtkCubeAxesActor2D()
    axes.SetCamera(ren.GetActiveCamera())
    axes.SetLabelFormat("%6.4g")
    axes.SetFlyModeToOuterEdges()
    axes.SetFontFactor(1)
    axes.SetBounds(-120, 120, -120, 120, 0, 100)
    ren.AddViewProp(axes)

    ren.SetBackground(0.6, 0.6, 0.8)
    ren.AddActor(actPlane)
    ren.AddActor(actCircle)

    ren.GetActiveCamera().SetPosition(9.0771443932283304, -566.72506794105004, 247.8299526806523)
    ren.GetActiveCamera().SetViewUp(-0.0011343377745808704, 0.54348693673594728, 0.83941685882235506)
    ren.GetActiveCamera().SetFocalPoint(-3.6197621940835187, -36.197621940833855, 47.5)

    sensor_file = '../datasheets/sensors/' + 'IRDistanceSensor.xml'
    IRSensor = MySimulatedIRSensor(sensorDataSheet=sensor_file,
                                               position=[],
                                               orientation=[]) 

    channel_file='../datasheets/events/'+"Dist_IRe.xml"
    SAS = MyAbstractSensorNode(outputDataSheet=channel_file,
                                 sensor_interface=IRSensor,
                                 appName="SAS Test",
                                 logger=[])
    
    position=[1,2,3]*quantities.centimeter
    SAS.setPosition(position)
    IRSensor.set_position(position.magnitude)
    orientation=[0,0,0]*quantities.degree
    SAS.setOrientation(orientation)
    IRSensor.set_orientation(orientation.magnitude)    
    beamMinMax=IRSensor.properties.getBeamMinMax()
    #SAS.startMeasurements()

    actSensorTrans=SAS.initSensorBeam()
    actSensorTrans.GetProperty().SetColor(red)
    ren.AddActor( actSensorTrans)  
 
    actSensor=SAS.initSensorBeam()
    ren.AddActor( actSensor) 
 
    srcMeas = vtk.vtkSphereSource()
    srcMeas.SetRadius(10)   
    mapMeas=vtk.vtkPolyDataMapper();
    mapMeas.SetInput( srcMeas.GetOutput() )       
    actMeas = vtk.vtkActor()
    actMeas.SetMapper(mapMeas)
    measurement=[0,0,100]
    actMeas.SetPosition(measurement)
    actMeas.GetProperty().SetColor(blue)
    ren.AddActor(actMeas)
    
    iren.AddObserver("KeyPressEvent", Keypress)
        
    print "Use [x,X, y,Y z,Z] to move the \"measurement\""
    print "represented by the blue sphere. According to the position"
    print "a the measurement is located inside or outside of the beam"
    print "\nPress e to exit"
    
    iren.Start()  
    print "Aus Maus"